
/*****************************************************************************\
 *                           Armature.cpp
\*****************************************************************************/

/*! @file 
 *
 *  @brief
 *
 *  @author Weiyu Yi, inweiyu@googlemail.com
 */

//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

#include <cassert>

#include <defEVectorMath.h>
#include <defEException.h>

#include "Armature.h"

namespace vhand
{

//***************************************************************************
//                              LimbRef
//***************************************************************************

LimbRef::LimbRef()
:
    _id( -1 ),
    _name(),
    //_bones_offset( 0 ),
    //_bones_size( 0 ),
    //_skin_offset( 0 ),
    //_skin_size( 0 ),
    //_spheres_offset( 0 ),
    //_spheres_size( 0 ),
    _rotmat(),
    _trlmat(),
    _id_children(),
    _id_parent( -1 )
{
}

LimbRef::LimbRef( const LimbRef &source )
:
    _id( source._id ),
    _name( source._name ),
    _rotmat( source._rotmat ),
    _trlmat( source._trlmat ),
    _id_children( source._id_children ),
    _id_parent( source._id_parent )
{
}

LimbRef::~LimbRef()
{
}

LimbRef& LimbRef::operator = ( const LimbRef &source )
{
    if ( this != &source )
    {
        _id = source._id;
        _name = source._name;
        _rotmat = source._rotmat;
        _trlmat = source._trlmat;
        _id_children = source._id_children;
        _id_parent = source._id_parent;
    }
}

void LimbRef::setRotation( const Quat &quat )
{
    _rotmat = Matrix4::rotation( quat );
}

void LimbRef::setRotation( const Matrix4 &rotmat )
{
    _rotmat = rotmat;
}

void LimbRef::setTranslation( const Vector3 &trlvec )
{
    _trlmat = Matrix4::translation( trlvec );
}

void LimbRef::rotate( const Matrix4 &rotmatnew )
{
    // both are rotation matrix
    // the sequence of multiplication doesn't play any role here
    _rotmat = _rotmat * rotmatnew;
}

void LimbRef::print( FILE *file/* = stdout */) const
{
    fprintf( file, "id: \r\t\t%d\nname: \r\t\t%s\nbones_offset: \r\t\t%d\nbones_size: \r\t\t%d\n",
             _id, _name.c_str(),
            _bonesRef.getArray()->front().first/*_bones_offset*/,
            _bonesRef.getArray()->front().second/*_bones_size*/ );
    fprintf( file, "parent   id: \r\t\t%d\n", _id_parent );
    fprintf( file, "children id: \r\t\t" );
    for ( unsigned int i=0; i < _id_children.size(); i ++ )
        fprintf( file, "%d, ", _id_children[i] );
    fprintf( file, "\n" );
}

//***************************************************************************
//                              Armature
//***************************************************************************

Armature::Armature()
:
    _num_limbs( 0 ),
    _alllimbrefs(),
    _limb_str2id(),
    _limb_id2str(),
    _update_mat(),
    _bones_update_range(),
    _skin_update_range(),
    _spheres_update_range(),
    _geom_coordsys()
{
}

/** Get the Coordinate Transformation matrix form the current anchor to root
 *
 * With the returned matrix, the coordinate wrt. root can be computed with
 * multiplication 
 *
 *      coord(v)_root = matrix * coord(v)_local
 * 
 * @param limb
 * 
 * @return 
 */
Matrix4 Armature::getTransformationToRoot( LimbRef limb ) const
{
    Matrix4 mat_star = Matrix4::identity();

    int id_tmp = limb.getID();
    while ( id_tmp != -1 )
    {
        mat_star = _alllimbrefs[id_tmp].getTransformation() * mat_star;
        id_tmp = _alllimbrefs[id_tmp].getParentID();
    }
    return mat_star;
}

/** 
 * 
 * @param limbname
 * 
 * @return 
 */
int Armature::findLimbID( std::string limbname )
{
    if ( _limb_str2id.find( limbname ) == _limb_str2id.end() )
    {
        throw XError( "Errors while calling %s : %s:\n\tThe limb %s hasn't been registered before.\n",
                      __FILE__, __FUNCTION__, limbname.c_str() );
    }

    int limbid = _limb_str2id[ limbname ];
    return limbid;
}

/** 
 * 
 * @param id
 * 
 * @return 
 */
std::string Armature::findLimbName( int id )
{
    if ( _limb_id2str.find( id ) == _limb_id2str.end() )
    {
        throw XError( "Errors while calling \"%s:%s\":\n\tThe limb with ID %d hasn't been registered before.\n",
                      __FILE__, __FUNCTION__,
                      id );
    }

    std::string name = _limb_id2str[ id ];
    return name;
}

/** Update the limb transformation matrix
 * 
 * @param std::string   The name of the limb
 * @param limbmat       The transformation matrix of limb wrt. parent limb
 */
void Armature::moveLimb( std::string limbname, const Matrix4 &limbmat )
{
    int limbid = findLimbID( limbname );
    moveLimb( limbid, limbmat );
}


/** Update the limb transformation matrix
 *
 * @param std::string   The id of the limb
 * @param tran          The transformation matrix of limb wrt. parent limb
 *
 * @todo 
 *      moveing root should be allowd.
 *
 */
void Armature::moveLimb( int limbid, const Matrix4 &limbmat )
{
    ////////////////////////////////////////////////////////////////////////////
    //
    // first find the limbref
    if ( !( limbid < _alllimbrefs.size() ) )
    {
        throw XError( "Errors while calling \"%s:%s\":\n\tThe limbref with ID %d hasn't been registered before.\n",
                      __FILE__, __FUNCTION__,
                      limbid );
    }
    int id_found = limbid;
    LimbRef found = _alllimbrefs[ id_found ];

    ////////////////////////////////////////////////////////////////////////////
    //
    // set the transformation matrix of anchor "star_found"
    //
    Matrix4 mat_star_found = getTransformationToRoot( found );
    Matrix4 mat_star_found_inverse = inverse( mat_star_found );

    // set the update matrix
    _update_mat = Matrix4::identity();
    _update_mat = limbmat * mat_star_found_inverse;
    _update_mat = mat_star_found * _update_mat;

    // update the rotated limbref
    /**
     * @care.
     *      found is the local tmp object, with it the really limbs in
     *      _alllimbrefs cannot be changed.
     */
    _alllimbrefs[ id_found ].rotate( limbmat );
    // found.rotate( limbmat ); // ERROR!

    ////////////////////////////////////////////////////////////////////////////
    //
    // set the update range
    // this function will recursively be called with the tree structure
    //
    /**
     * @care.
     *      _update_range must be cleared each time before updateRange() is called
     */
    _bones_update_range.clear(); // Care! it must be cleared everytime
    _skin_update_range.clear();
    _spheres_update_range.clear();
    updateRange( id_found );

    ////////////////////////////////////////////////////////////////////////////
    //
    // the elements in _updata_range may be continuous/successive, so they
    // could be combined, in order that there could be less update ranges.
    //
    // Todo...
    
    return;
}


/** 
 * 
 * @param limbid
 */
void Armature::updateRange( int limbid )
{
    if ( !( limbid < _alllimbrefs.size() ) )
    {
        throw XError( "Errors while calling \"%s:%s\":\n\tThe limbref with ID %d hasn't been registered before.\n",
                      __FILE__, __FUNCTION__,
                      limbid );
    }
    const LimbRef &limb = _alllimbrefs[ limbid ];

#if 0
    _bones_update_range  .push_back( std::pair< int, int >( limb.getBonesOffset(),
                                                            limb.getBonesSize() ) );
    _skin_update_range   .push_back( std::pair< int, int >( limb.getSkinOffset(),
                                                            limb.getSkinSize() ) );
    _spheres_update_range.push_back( std::pair< int, int >( limb.getSpheresOffset(),
                                                            limb.getSpheresSize() ) );
#endif

    for ( std::vector<IndexRange>::const_iterator iter = limb.getBonesRef()->getArray()->begin();
          iter != limb.getBonesRef()->getArray()->end(); iter ++ )
    {
        _bones_update_range.addRange( *iter );
    }
    for ( std::vector<IndexRange>::const_iterator iter = limb.getSkinRef()->getArray()->begin();
          iter != limb.getSkinRef()->getArray()->end(); iter ++ )
    {
        _skin_update_range.addRange( *iter );
    }
    for ( std::vector<IndexRange>::const_iterator iter = limb.getSpheresRef()->getArray()->begin();
          iter != limb.getSpheresRef()->getArray()->end(); iter ++ )
    {
        _spheres_update_range.addRange( *iter );
    }

    const std::vector< int > *cids = limb.getChildrenIDs();
    for ( unsigned int i=0; i < cids->size(); i++ )
    {
        updateRange( (*cids)[i] );        
    }
}


void Armature::makeLimbBonesRef( void )
{
    printf( "Now it shouldn't be called.\n" );
    printf( "todo...\n\tRewrite the loading process of bones' geometry and armature.\n" );
}

void Armature::makeLimbSkinRef( void )
{
    bool range_valid = false;
    IndexRange tmprange(0,0);
    int tmpid = -1;
    for ( std::map<int,int>::const_iterator iter = _skin2limb.begin();
          iter != _skin2limb.end(); iter ++ )
    {
        // this range belongs to the same LimbRef object
        if ( iter->second == tmpid )
        {
            assert( range_valid == true );

            // the new index belongs to the existing range
            if ( iter->first == ( tmprange.first + tmprange.second ) )
            {
                tmprange.second += 1;
            }
            else
            {
                // add this range into the corresponding limbref first
                _alllimbrefs[ tmpid ].editSkinRef()->addRange( tmprange );
                // then make new range
                tmprange = IndexRange( iter->first, 1 );
                range_valid = true;
            }
        }
        // otherwise
        else
        {
            // if there is already a processing range
            if ( range_valid == true )
            {
                // add this range into the corresponding limbref first
                _alllimbrefs[ tmpid ].editSkinRef()->addRange( tmprange );
            }
            // then make tmpid new value
            tmpid = iter->second;
            // then make new range
            tmprange = IndexRange( iter->first, 1 );
            range_valid = true;
        }
    }
    if ( range_valid == true )
    {
        // add this range into the corresponding limbref
        _alllimbrefs[ tmpid ].editSkinRef()->addRange( tmprange );
    }
}

void Armature::makeLimbSpheresRef( void )
{
    bool range_valid = false;
    IndexRange tmprange(0,0);
    int tmpid = -1;
    for ( std::map<int,int>::const_iterator iter = _spheres2limb.begin();
          iter != _spheres2limb.end(); iter ++ )
    {
        // this range belongs to the same LimbRef object
        if ( iter->second == tmpid )
        {
            assert( range_valid == true );

            // the new index belongs to the existing range
            if ( iter->first == ( tmprange.first + tmprange.second ) )
            {
                tmprange.second += 1;
            }
            else
            {
                // add this range into the corresponding limbref first
                _alllimbrefs[ tmpid ].editSpheresRef()->addRange( tmprange );
                // then make new range
                tmprange = IndexRange( iter->first, 1 );
                range_valid = true;
            }
        }
        // otherwise
        else
        {
            // if there is already a processing range
            if ( range_valid == true )
            {
                // add this range into the corresponding limbref first
                _alllimbrefs[ tmpid ].editSpheresRef()->addRange( tmprange );
            }
            // then make tmpid new value
            tmpid = iter->second;
            // then make new range
            tmprange = IndexRange( iter->first, 1 );
            range_valid = true;
        }
    }
    if ( range_valid == true )
    {
        // add this range into the corresponding limbref
        _alllimbrefs[ tmpid ].editSpheresRef()->addRange( tmprange );
    }
}


/** 
 * 
 * @return 
 */
int Armature::initGraphicsResource( void )
{
    _geom_coordsys = CoordSys( 0.1 );
    _geom_coordsys.initGraphicsResource();
    return 0;
}


/** 
 * 
 * @return 
 */
int Armature::draw( void ) const
{
    for ( unsigned int k=0; k<_alllimbrefs.size(); k++ )
    {
        LimbRef limb = _alllimbrefs[k];
        Matrix4 mat_limb = getTransformationToRoot( limb );

        // go on with OpenGL context
        GLdouble mat_GL[16];
        glPushMatrix();
        for ( unsigned int i=0; i<4; i++ )
            for ( unsigned int j=0; j<4; j++ )
                mat_GL[ i*4+j ] = mat_limb[i][j];
        glMultMatrixd( mat_GL );
        _geom_coordsys.draw();
        glPopMatrix();
    }

    return 0;
}

/** Update the limb transformation matrix
 *
 * @param std::string   The id of the limb
 * @param tran          The transformation matrix of limb wrt. parent limb
 *
 * @todo 
 *      moveing root should be allowd.
 *
 */
//void Armature::moveLimb( int limbid, Matrix4 limbmat )
//{
    //////////////////////////////////////////////////////////////////////////////
    ////
    //// first find the limbref
    //if ( !( limbid < _alllimbrefs.size() ) )
    //{
        //throw XError( "Errors while calling \"%s:%s\":\n\tThe limbref with ID %d hasn't been registered before.\n",
                      //__FILE__, __FUNCTION__,
                      //limbid );
    //}

    //// find the parent
    //int id_found = limbid;
    //LimbRef found = _alllimbrefs[ limbid ];
    //found.print();

    /**
     * @todo 
     *      moveing root should be allowd.
     */
    //if ( found.isRoot() )
    //{
        //throw XError( "Errors while calling \"%s:%s\":\n\tIf root of armature is moved, move the whole geometry instead\n",
                      //__FILE__, __FUNCTION__ );
    //}

    //int id_found_parent = found.getParentID();
    //if ( !( id_found_parent < _alllimbrefs.size() ) )
    //{
        //throw XError( "Errors while calling \"%s:%s\":\n\tThis parent limbref with ID %d hasn't been registered before.\n",
                      //__FILE__, __FUNCTION__,
                      //id_found_parent );
    //}
    //LimbRef found_parent = _alllimbrefs[ id_found_parent ];

    //////////////////////////////////////////////////////////////////////////////
    ////
    //// set the transformation matrix of anchor "star_found_parent"
    //Matrix4 mat_star_found_parent = Matrix4::identity();
    //int id_tmp_parent = found_parent.getID();
    //while( !( _alllimbrefs[ id_tmp_parent ].isRoot() ) )
    //{
        //mat_star_found_parent = _alllimbrefs[id_tmp_parent].getMatrix() * mat_star_found_parent;
        //id_tmp_parent = _alllimbrefs[ id_tmp_parent ].getParentID();
    //}

    //// set the transformation matrix of anchor "found"
    //Matrix4 mat_found = found.getMatrix();

    //// set the transformation matrix of anchor "star_found"
    //Matrix4 mat_star_found = mat_star_found_parent * mat_found;
    //Matrix4 mat_star_found_inverse = inverse( mat_star_found );

    //// set the new transformation matrix of anchor "found"
    ////found.rotate( limbmat );
    ////Matrix4 mat_found_new = found.getMatrix();
    ////Matrix4 mat_found_new = limbmat * found.getMatrix();

    ////Matrix4 mat_found_new = found.getMatrix() * limbmat;
    ////Matrix4 mat_found_new = found.getTranslate() * limbmat * found.getTranslate();

    //// set the update matrix
    //_update_mat = mat_star_found_parent * mat_found_new * mat_star_found_inverse;

    //////////////////////////////////////////////////////////////////////////////
    ////
    //// set the update range
    //// this function will recursively be called with the tree structure
    //updateRange( id_found );
//}

} // namespace vhand
